/*
 * File:    ADC.c
 * Project:	HobbyPilotFirmware (HPF)
 * Author:  Wouter van Verre
 *
 * Created on January 20, 2011, 3:09 PM
 */

#include "Header.h"

void initADC ( void )
{	
	// Set up the ADCON1 register
	ADCON1bits.ADON = 0; 			// Turn off the ADC module while changing the registers
	ADCON1bits.ADSIDL = 1; 			// Stop the ADC module when the CPU enters idle mode
 	ADCON1bits.FORM = 0b01;			// Return signed integers
    ADCON1bits.SSRC = 0b111;		// Automatically start the conversion
    ADCON1bits.ASAM = 1;			// Automatically start sampling

    // Set up the ADCON2 register
	ADCON2bits.VCFG = 0b001;		// Use Vref+ pin as VrefH; Use AGND as VrefL
	ADCON2bits.CSCNA = 1;			// Scan channel 0
	ADCON2bits.SMPI = 0b0110;
	ADCON2bits.BUFM = 0;			// Use one 16bit word
	ADCON2bits.ALTS = 0;			// Use only MUX A inputs

	// Set up the ADCON3 register
	ADCON3bits.ADCS	= 63;			// 10kHz sample frequency @ TSMP = TAD and Fcy = 14.74 Mhz
	ADCON3bits.SAMC = 31;			// Auto Sample Time = 1*TAD
	ADCON3bits.ADRC = 0;			// Don't use the internal RC

	// Set up the ADCHS register
	ADCHS = 0b0000000000000001;		// Set all the values in this register to 0
           
	// Set up the ADPCFG register
	ADPCFG = 0b0000001111110000;	// Set up AN1,2,3 & AN10,11,12
	
	// Set up the ADCSSL register
	ADCSSL = 0b0001110000001111; 	// Set up AN1,2,3 & AN10,11,12

	ADCON1bits.ADON = 1;			// Turn the ADC module back on
}

void __attribute__((__interrupt__, __no_auto_psv__)) _ADCInterrupt (void)
{
	EnterInterrupt();

	XRate.input = XRateSign(XRateBuff);
	YRate.input = YRateSign(YRateBuff);
	ZRate.input = ZRateSign(ZRateBuff);
	XAccel.input = XAccelSign(XAccelBuff);
	YAccel.input = YAccelSign(YAccelBuff);
	ZAccel.input = ZAccelSign(ZAccelBuff);

	if (CPUStatus.readADC)
	{
		XRate.accum = 0;
		YRate.accum = 0;
		ZRate.accum = 0;
		XAccel.accum = 0;
		YAccel.accum = 0;
		ZAccel.accum = 0;

		ADCSampleCount = 0;
		CPUStatus.readADC = 0;
	}

	if (!CPUStatus.readingADC)
	{
		XRate.accum += XRate.input;
		YRate.accum += YRate.input;
		ZRate.accum += ZRate.input;
		XAccel.accum += XAccel.input;
		YAccel.accum += YAccel.input;
		ZAccel.accum += ZAccel.input;
		
		ADCSampleCount++;
	}
	
	IFS0bits.ADIF = 0;	// Clear the AD interrupt flag
}

void readSensorChannels(void)
{
	CPUStatus.readingADC = 1;

	if (initCounter == 0)
	{
		XRate.value = (__builtin_divsd(XRate.accum, ADCSampleCount) - XRate.offset);
		YRate.value = (__builtin_divsd(YRate.accum, ADCSampleCount) - YRate.offset);
		ZRate.value = (__builtin_divsd(ZRate.accum, ADCSampleCount) - ZRate.offset);

		XAccel.value = (__builtin_divsd(XAccel.accum, ADCSampleCount) - XAccel.offset);
		YAccel.value = (__builtin_divsd(YAccel.accum, ADCSampleCount) - YAccel.offset);
		ZAccel.value = (__builtin_divsd(ZAccel.accum, ADCSampleCount) - ZAccel.offset);
	}
	else
	{
		XRate.value = (__builtin_divsd(XRate.accum, ADCSampleCount));
		YRate.value = (__builtin_divsd(YRate.accum, ADCSampleCount));
		ZRate.value = (__builtin_divsd(ZRate.accum, ADCSampleCount));

		XAccel.value = (__builtin_divsd(XAccel.accum, ADCSampleCount));
		YAccel.value = (__builtin_divsd(YAccel.accum, ADCSampleCount));
		ZAccel.value = (__builtin_divsd(ZAccel.accum, ADCSampleCount));
	}

	CPUStatus.readingADC = 0;
	CPUStatus.readADC = 1;
}
